#!/bin/bash

# ==========================================
# Ubuntu 24.04 PX4 + ROS 2 Jazzy 全栈部署脚本
# 作者: Cross
# 版本: 2025.11 (适配 Ubuntu 24.04)
# ==========================================

set -e  # 遇到错误立即停止

# 颜色定义
GREEN='\033[0;32m'
BLUE='\033[0;34m'
RED='\033[0;31m'
NC='\033[0m'

log() { echo -e "${BLUE}[DEPLOY] $1${NC}"; }
success() { echo -e "${GREEN}[SUCCESS] $1${NC}"; }

# 0. 基础环境准备
log "步骤 0/6: 更新系统并安装基础依赖..."
sudo apt update && sudo apt upgrade -y
sudo apt install -y software-properties-common curl gnupg2 lsb-release git wget python3-pip vim

# 启用 Universe 仓库 (QGC 依赖 libfuse2 在这里)
sudo add-apt-repository universe -y

# Independent python/pip


# 1. 安装 ROS 2 Jazzy Jalisco
log "步骤 1/6: 安装 ROS 2 Jazzy..."
if ! command -v ros2 &> /dev/null; then
    sudo curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key -o /usr/share/keyrings/ros-archive-keyring.gpg
    echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/ros-archive-keyring.gpg] http://packages.ros.org/ros2/ubuntu $(source /etc/os-release && echo $UBUNTU_CODENAME) main" | sudo tee /etc/apt/sources.list.d/ros2.list > /dev/null
    sudo apt update
    sudo apt install -y ros-jazzy-desktop ros-dev-tools
    
    # 初始化 rosdep
    if [ ! -f /etc/ros/rosdep/sources.list.d/20-default.list ]; then
        sudo rosdep init
    fi
    rosdep update
    
    # 安装 ROS-Gazebo 桥接包
    sudo apt install -y ros-jazzy-ros-gz
else
    success "ROS 2 似乎已安装，跳过。"
fi

# 2. 安装 PX4 Autopilot (最新 Main 分支)
log "步骤 2/6: 部署 PX4 Autopilot..."
cd ~
if [ ! -d "PX4-Autopilot" ]; then
    git clone https://github.com/PX4/PX4-Autopilot.git --recursive
    cd PX4-Autopilot
    # 使用官方脚本安装依赖，跳过它自带的旧版 ROS 安装，只安装工具链
    # 注意：我们使用 --no-nuttx 先不编译，仅安装环境
    bash ./Tools/setup/ubuntu.sh --no-nuttx --no-sim-tools
    
    # 手动补全 Python 依赖 (Ubuntu 24.04 PEP 668 限制，使用 --break-system-packages 或虚拟环境，这里为了全局兼容性安装必要的系统包)
    # tomllib is not needed for python 13.
    pip3 install --user -U kconfiglib jsonschema jinja2 pyros-genmsg packaging future numpy pandas
    # OR
    pip3 install --user -U empy pyros-genmsg setuptools
else
    log "PX4 目录已存在，尝试更新..."
    cd ~/PX4-Autopilot
    git pull
    git submodule update --init --recursive
fi

# 3. 安装 Micro-XRCE-DDS Agent (ROS2 通信桥梁)
log "步骤 3/6: 安装 Micro-ROS Agent..."
cd ~
if [ ! -d "Micro-XRCE-DDS-Agent" ]; then
    git clone https://github.com/eProsima/Micro-XRCE-DDS-Agent.git
    cd Micro-XRCE-DDS-Agent
    mkdir build && cd build
    cmake ..
    make
    sudo make install
    sudo ldconfig /usr/local/lib/
fi

# 4. 安装 QGroundControl (地面站) for ubuntu x86
log "步骤 4/6: 安装 QGroundControl..."
cd ~
# Ubuntu 24.04 需要 libfuse2 才能运行 AppImage
sudo usermod -a -G dialout $USER
sudo apt-get remove modemmanager -y
sudo apt install gstreamer1.0-plugins-bad gstreamer1.0-libav gstreamer1.0-gl -y
sudo apt install libfuse2 -y
sudo apt install libxcb-xinerama0 libxkbcommon-x11-0 libxcb-cursor-dev -y

if [ ! -f "QGroundControl.AppImage" ]; then
    wget https://d176tv9ibo4jno.cloudfront.net/latest/QGroundControl-x86_64.AppImage
    mv QGroundControl-x86_64.AppImage QGroundControl.AppImage
    chmod +x QGroundControl.AppImage
fi

# 5. 安装高级绘图与地图工具
log "步骤 5/6: [Optional]安装 PlotJuggler 和 QGIS..."
# # PlotJuggler (强大的日志分析工具，ROS 友好)
# sudo apt install -y ros-jazzy-plotjuggler-ros

# # QGIS (专业的开源 GIS 地图工具)
# sudo apt install -y qgis qgis-plugin-grass

# 6. 环境配置
log "步骤 6/6: 配置环境变量..."
SHELL_CONFIG="$HOME/.bashrc"

# 添加 ROS 2 环境
if ! grep -q "source /opt/ros/jazzy/setup.bash" "$SHELL_CONFIG"; then
    echo "source /opt/ros/jazzy/setup.bash" >> "$SHELL_CONFIG"
fi

# 添加 PX4 环境
if ! grep -q "PX4-Autopilot" "$SHELL_CONFIG"; then
    echo "export PATH=\$PATH:$HOME/PX4-Autopilot/build/px4_sitl_default/bin" >> "$SHELL_CONFIG"
fi

### for zsh
# SHELL_CONFIG="$HOME/.zshrc"

# # 添加 ROS 2 环境
# if ! grep -q "source /opt/ros/jazzy/setup.bash" "$SHELL_CONFIG"; then
#     echo "source /opt/ros/jazzy/setup.zsh" >> "$SHELL_CONFIG"
# fi

# # 添加 PX4 环境
# if ! grep -q "PX4-Autopilot" "$SHELL_CONFIG"; then
#     echo "export PATH=\$PATH:$HOME/PX4-Autopilot/build/px4_sitl_default/bin" >> "$SHELL_CONFIG"
# fi


success "========================================================"
success "  全栈环境部署完成！"
success "  请务必执行: source ~/.bashrc 或者 重启电脑"
success "========================================================"
